#include <librealsense2/rs.hpp>
#include <opencv2/opencv.hpp>
#include <chrono>
#include <thread>
#include <iostream>

int main()
{

    // 设置hsv阈值
    cv::Scalar lower_blue(100, 43, 46);
    cv::Scalar upper_blue(124, 255, 255);
    cv::Scalar lower_red(0, 43, 20);
    cv::Scalar upper_red(10, 255, 255);
    // Create a pipeline object
    rs2::pipeline pipeline;
    // Configure the streams
    rs2::config config;
    config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_BGR8, 30);
    config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
    std::cout << "start:"<<std::endl;

    // 左右双目
    // Enable stereo infrared streams
    config.enable_stream(RS2_STREAM_INFRARED, 1, 640, 480, RS2_FORMAT_Y8, 30);
    config.enable_stream(RS2_STREAM_INFRARED, 2, 640, 480, RS2_FORMAT_Y8, 30);
    // Create an align object to align depth and color frames
    rs2::align align(RS2_STREAM_COLOR);
    // Start the pipeline
    rs2::pipeline_profile profile = pipeline.start(config);
    // 更改rgb相机的曝光值
    rs2::color_sensor color_sensor = profile.get_device().first<rs2::color_sensor>();
    color_sensor.set_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0);


    // 设置曝光值
    color_sensor.set_option(RS2_OPTION_EXPOSURE, 25);
    // Get the depth sensor and the depth scale
    rs2::depth_sensor depth_sensor = profile.get_device().first<rs2::depth_sensor>();
    float depth_scale = depth_sensor.get_depth_scale();

    // Define the parameters for filtering big contours
    bool useRatio = true;
    int useRatioNum = 0;
    int big_area = 2000;

    // 对视频进行循环处理
    while (true)
    {
        // Get frameset of color and depth
        rs2::frameset frames = pipeline.wait_for_frames();
        frames = align.process(frames);
        rs2::video_frame color_frame = frames.get_color_frame();
        // 获得深度图像
        rs2::depth_frame depth_frame = frames.get_depth_frame();
        if (!color_frame || !depth_frame)
        {
            continue;
        }
        
        // Convert images to cv::Mat objects
        cv::Mat color_image(cv::Size(640, 480), CV_8UC3, (void *)color_frame.get_data(), cv::Mat::AUTO_STEP);
        cv::Mat depth_image(cv::Size(640, 480), CV_16U, (void *)depth_frame.get_data(), cv::Mat::AUTO_STEP);
        cv::Mat frame = color_image;
        cv::Mat depth_colormap;
        cv::convertScaleAbs(depth_image, depth_image, 0.21); //
        cv::applyColorMap(depth_image, depth_colormap, cv::COLORMAP_JET); //

        // 放慢视频播放速度
        // std::this_thread::sleep_for(std::chrono::milliseconds(200));

        // 转换到HSV
        cv::Mat hsv;
        cv::cvtColor(frame, hsv, cv::COLOR_BGR2HSV);

        // 根据阈值构建掩模
        cv::Mat binary_p;
        cv::inRange(hsv, lower_red, upper_red, binary_p);

        // 对原图像和掩模进行位运算
        // cv::imshow("binary", binary);

        // 对图像进行形态学处理
        cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5));
        cv::Mat binary;
        cv::morphologyEx(binary_p, binary, cv::MORPH_OPEN, kernel);
        cv::imshow("binary", binary);
        // 显示图像
        cv::imshow("frame", frame);
        // cv::imshow("depth", depth_colormap);

        // Wait for a key press and exit if 'q' is pressed
        if (cv::waitKey(1) == 'q')
        {
            break;
        }

    }
    // cap.release();
    // cv::destroyAllWindows();
}